#include "basicPhysics.h"
#include <iostream>
///btBulletDynamicsCommon.h is the main Bullet include file, contains most common include files.
#include "btBulletDynamicsCommon.h"
#include <stdio.h> //printf debugging
#include "BulletDynamics/ConstraintSolver/btConstraintSolver.h"
#include "BulletDynamics/Dynamics/btDynamicsWorld.h"
#include <irrlicht.h>

using namespace std;

///create 125 (5x5x5) dynamic object
#define ARRAY_SIZE_X 5
#define ARRAY_SIZE_Y 5
#define ARRAY_SIZE_Z 5

//maximum number of objects (and allow user to shoot additional boxes)
#define MAX_PROXIES (ARRAY_SIZE_X*ARRAY_SIZE_Y*ARRAY_SIZE_Z + 1024)

///scaling of the objects (0.1 = 20 centimeter boxes )
#define SCALING 0.1
#define START_POS_X -5
#define START_POS_Y -5
#define START_POS_Z -3

bool DEBUG = true;

void	basicPhysics::initPhysics()
{
	if (DEBUG == true)	cout<<"initPhysics() called"<<endl;

	///collision configuration contains default setup for memory, collision setup
	m_collisionConfiguration = new btDefaultCollisionConfiguration();
	//m_collisionConfiguration->setConvexConvexMultipointIterations();

	///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded)
	m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);

	m_broadphase = new btDbvtBroadphase();

	///the default constraint solver. For parallel processing you can use a different solver (see Extras/BulletMultiThreaded)
	btSequentialImpulseConstraintSolver* sol = new btSequentialImpulseConstraintSolver;
	m_solver = sol;

	m_dynamicsWorld = new btDiscreteDynamicsWorld(m_dispatcher,m_broadphase,m_solver,m_collisionConfiguration);

	m_dynamicsWorld->setGravity(btVector3(0,-10,0));

	///create a few basic rigid bodies
	btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(50.),btScalar(50.),btScalar(50.)));
//	btCollisionShape* groundShape = new btStaticPlaneShape(btVector3(0,1,0),50);
	
	m_collisionShapes.push_back(groundShape);

	btTransform groundTransform;
	groundTransform.setIdentity();
	groundTransform.setOrigin(btVector3(0,0,100));

	//We can also use DemoApplication::localCreateRigidBody, but for clarity it is provided here:
	{
		btScalar mass(0.1);

		//rigidbody is dynamic if and only if mass is non zero, otherwise static
		bool isDynamic = (mass != 0.f);

		btVector3 localInertia(0,0,0);
		if (isDynamic)
			groundShape->calculateLocalInertia(mass,localInertia);

		//using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
		btDefaultMotionState* myMotionState = new btDefaultMotionState(groundTransform);
		btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,groundShape,localInertia);
		// btRigidBody* 
		body = new btRigidBody(rbInfo);
		body_2 = new btRigidBody(rbInfo);
		
		//add the body to the dynamics world
		 m_dynamicsWorld->addRigidBody(body);
		m_dynamicsWorld->addRigidBody(body_2);

		// Create a hinge and constraint
		const btVector3 pivotPoint( btScalar(0) , btScalar(100),btScalar(0));
		
		bool useReferenceFrameA = false;
		btVector3 axisBody(0,0,1);
		
		btHingeConstraint* ct = new btHingeConstraint(*body,pivotPoint,axisBody,useReferenceFrameA);

		btTransform localA, localB;

		localA.setIdentity(); 
		localB.setIdentity();
		localA.getBasis().setEulerZYX(0,0,0); 
		localA.setOrigin(btVector3(btScalar(0), btScalar(0), btScalar(0)));
		localB.getBasis().setEulerZYX(0,0,0); 
		localB.setOrigin(btVector3(btScalar(110), btScalar(110), btScalar(0)));

		// body->setCenterOfMassTransform(localA);
		// body_2->setCenterOfMassTransform(localB);

		btHingeConstraint* ct_2 = new btHingeConstraint(*body, *body_2,localA,localB,useReferenceFrameA);
		// ct_2->setLimit(0.5,0.5,0.5);
		// add constraint to the world

		// body_2->setGravity(btVector3(0,-10,0));
		// body_2->applyGravity();
		
		//set some damping

		body->setDamping(0.005, 0.085);
		body_2->setDamping(0.005, 0.085);

		m_dynamicsWorld->addConstraint(ct);
	 	//m_dynamicsWorld->addRigidBody(body);

		m_dynamicsWorld->addConstraint(ct_2);
	 	//m_dynamicsWorld->addRigidBody(body_2);
		
	}


};

void basicPhysics::getPhysicsPos()
{
	if (DEBUG == true)
	cout<<"getPhysicsPos() called"<<endl;

	 //body->getCenterOfMassPosition();
	 btVector3 testVector = body->getCenterOfMassPosition();
	if (DEBUG == true)
	{
	const f32 x = testVector[0];
	const f32 y = testVector[1];
	const f32 z = testVector[2]; 

	 cout<<"btVector <X Y Z> is "<< x <<" "<< y << " "<< z<<endl;
	}
};

void	basicPhysics::exitPhysics()
{

	//cleanup in the reverse order of creation/initialization

	//remove the rigidbodies from the dynamics world and delete them
	int i;
	for (i=m_dynamicsWorld->getNumCollisionObjects()-1; i>=0 ;i--)
	{
		btCollisionObject* obj = m_dynamicsWorld->getCollisionObjectArray()[i];
		btRigidBody* body = btRigidBody::upcast(obj);
		if (body && body->getMotionState())
		{
			delete body->getMotionState();
		}
		m_dynamicsWorld->removeCollisionObject( obj );
		delete obj;
	}

	//delete collision shapes
//	for (int j=0;j<m_collisionShapes.size();j++)
//	{
//		btCollisionShape* shape = m_collisionShapes[j];
//		delete shape;
//	}

	delete m_dynamicsWorld;
	
	delete m_solver;
	
	delete m_broadphase;
	
	delete m_dispatcher;

	delete m_collisionConfiguration;

	
};

basicPhysics::basicPhysics()
{
};

basicPhysics::~basicPhysics()
{
};

